#include "encode.h"
#include "tim.h"

short Robot_Encoder_CNT[2];//编码器次数上溢出和下溢出次数
_Moto_Str Moto1, Moto2;
tracing_Str tracing;
Path_len Path_lenget= {0,0};
void Robot_Encoder_Start(void)
{
	TIM4->CNT = 0x7fff;
	TIM5->CNT = 0x7fff;
}

/*
 @ describetion: Get encoder value
 @ param:  NONE
 @ return: none
 @ author: Xuewei Zhou
 @ date : 2019-3-14
 @ function : void  Robot_Encoder_Get_CNT(void)
*/

void  Robot_Encoder_Get_CNT(void)
{
	Moto1.Encoder_Value  = (TIM4->CNT)-0x7fff + Robot_Encoder_CNT[0]*0xffff;		
	Moto2.Encoder_Value  = (TIM5->CNT)-0x7fff + Robot_Encoder_CNT[1]*0xffff;
    Robot_Encoder_CNT[0] = 0;
    Robot_Encoder_CNT[1] = 0;
	// 使用积分计算轮子里程
	Path_lenget.Left_len += -DX_TO_MOTOR(Moto1.Encoder_Value);
	Path_lenget.Right_len += DX_TO_MOTOR(Moto2.Encoder_Value);
	
	Moto1.Current_Speed = -CURRENT_MOTO_SPEED(Moto1.Encoder_Value);
	Moto2.Current_Speed = CURRENT_MOTO_SPEED(Moto2.Encoder_Value);
	// tracing.MOTO1_Current_Speed = Moto1.Current_Speed;
	// tracing.MOTO2_Current_Speed = Moto2.Current_Speed;

	Robot_Encoder_Start();			//清除左右轮子的脉冲数
}